#include "utils.h"
#include <assert.h>
#include <interface/msg/object.hpp>
#include <sstream>

namespace fusion_perception {

std::string printObject(const Object& object)
{
    std::stringstream ss;

    ss << "frameId:" << object.header.frame_id << ",id:" << object.id << ",x:" << object.x << ",y:" << object.y << ",type:" << object.type
       << ",heading:" << object.heading << ",speed:" << object.speed << ",speedX:" << object.speedx << ",speedY:" << object.speedy;

    return ss.str();
}

float cosineDist(const std::vector<float>& f1, const std::vector<float>& f2)
{
    assert(f1.size() == f2.size());

    double sum = 0;
    for (size_t i = 0; i < f1.size(); ++i) {
        sum += f1[i] * f2[i];
    }

    double dist = 1 - sum;

    return dist;
}

}
